/*
 * EKFKinematic.h
 *
 *  Created on: May 26, 2011
 *      Author: gbower
 */

#ifndef EKFKinematic_H_
#define EKFKinematic_H_

#define numStates 20
#define gpsSize 5
#define imuSize 5

// True magnetic field strength -> Stanford in 2011
// Should probably update to run world magnetic model code at initial location (Note: we don't have lat/lon location from autopilot now)
#define N_Mag_Field	0.470202056007494	// North magnetic field strength normalized to 1
#define E_Mag_Field	0.117782321161020	// East magnetic field strength normalized to 1
#define D_Mag_Field	0.874664136310760	// Down magnetic field strength normalized to 1


#include <cstdio>
#include <stdio.h>
#include <list>
#include <math.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include "Communication.h"
#include "execStruct.h"
#include "Matrix.h"
#include "Aircraft.h"
#include "mavlink.h"

struct stateVector {
	double Np, Ep, Dp;		// Inertial position
	double VNi, VEi, VDi;	// Inertial velocities
	double q0, q1, q2, q3; 	// Attitude quaternions
};

struct paramVector {
	double VNi_wind, VEi_wind; 			// Winds
	double wbx, wby, wbz, diffb, barob; // Sensor biases
	double psis, thetas, phis; 			// Sensor alignment angles (accelerometer1 and gyros)
};


class EKFKinematic: public Communication::AUPListener {

public:

	double dt, ts;
	Aircraft *ac;
	paramVector *pv;
	stateVector *sv;
	ofstream *Sfile;

	EKFKinematic(Aircraft* aci);
	virtual ~EKFKinematic();
	void AUPdataReceived(Communication &com);
	
	// Methods
	void initializeState();
	void getCurrentState(double *xcurr);
	void runProcessModel(double *x, double *mubt, double dt);
	void computeProcessJacobian(double *x, double *G, double dt);
	void quaternionUpdate(double *x, double wx, double wy, double wz, double dt) {
	void computePredictedVariance(double *G, double *Sbart);	// NEED TO CHECK/UPDATE
	void runMeasModel(double *x, double *zbt, bool flag);
	void computeMeasJacobian(double *x, double *H, bool flag);
	void computeKalmanGain(double *Sbart, double *H, double *Kt, bool flag);	// NEED TO CHECK/UPDATE
	void computeNewMeanandCovariance(double *mubt, double *Kt, double *zbt,	double *Sbart, double *Ht, bool flag);					
	
	void updateState(double *mut);

	void printData(double t);
	void closeFile();

private:

	// Initial Covariance Estimate
	double P[numStates*numStates]; 		// State covariance estimate
	double Q_IMU_GPS[gpsSize+imuSize];	// GPS + IMU measurement noise (uncorrelated)
	double Q_IMU[imuSize];				// IMU measurement noise (uncorrelated)
	double R[numStates]; 				// Process model noise (uncorrelated)

//	double Hs[numStates*(imuSize+gpsSize)];		// Measurement model jacobian
//	double Gs[numStates*numStates];				// Process model jacobian
};

#endif /* EKFKinematic_H_ */
